#pragma once

#include "camera_detectors.h"
#include "one_camera_tracker.h"

#include <rclcpp/rclcpp.hpp>

#include "lane_detector/lane_detector.h"
#include "reid/reid.h"
#include <interface/msg/images.hpp>
#include <interface/msg/multi_lane_points.hpp>
#include <interface/msg/object.hpp>
#include <interface/msg/objects.hpp>
#include <sensor_msgs/msg/image.hpp>
#include "reid/reid.h"
#include "lane_detector/lane_detector.h"
#include "lane_seg/lane_seg.h"
#include <unordered_map>

namespace perception::camera {

class CameraAdapter : public rclcpp::Node {
public:
    explicit CameraAdapter(std::string name);

private:
    void onImages(std::shared_ptr<interface::msg::Images> msg);

    void paint_on_images(std::unordered_map<std::string, cv::Mat>& images,
        std::unordered_map<std::string, std::vector<ObjectTrackInfoPtr>>& tracked);

    void paint_on_images(std::unordered_map<std::string, cv::Mat>& images,
        std::unordered_map<std::string, std::vector<ObjectDetectInfoPtr>>& detected);

    void paint_on_bev(std::unordered_map<std::string, cv::Mat>& images,
        std::unordered_map<std::string, std::vector<ObjectDetectInfoPtr>>& detected,
        std::unordered_map<std::string, std::vector<ObjectTrackInfoPtr>>& tracked);

    void send_tracked_objects(const builtin_interfaces::msg::Time& tsms,
        std::unordered_map<std::string, builtin_interfaces::msg::Time>& tsmss,
        const std::unordered_map<std::string, std::vector<ObjectTrackInfoPtr>>& tracked);

    // void convert_uv2xy(const std::string &cam_name, const std::vector<cv::Point2f> &im_pt, std::vector<cv::Point2f> &ground_pts);
    // void convert_uv2xy(const cv::Matx<double, 3, 3> &K, const cv::Matx<double, 1, 5> &distort,
    //                         const std::vector<cv::Point2f> &im_pt, std::vector<cv::Point2f> &ground_pts);
private:
    rclcpp::Logger logger_;
    std::map<std::string, cv::Scalar> cam_color = {
        { "camera0", cv::Scalar { 0, 0, 255 } },
        { "camera1", cv::Scalar { 0, 255, 0 } },
        { "camera2", cv::Scalar { 255, 0, 255 } },
        { "camera3", cv::Scalar { 128, 128, 0 } },
        { "camera4", cv::Scalar { 128, 0, 128 } },
        { "camera5", cv::Scalar { 0, 128, 128 } },
        { "camera6", cv::Scalar { 255, 255, 0 } }
    };

    std::shared_ptr<CameraDetectors> detectors_;
    std::shared_ptr<ReID> reid_;
    std::shared_ptr<LaneDetector> lane_detector_;
    std::shared_ptr<LaneSeg> lane_seg_;
    std::unordered_map<std::string, std::shared_ptr<OneCameraTracker>> trackers_;
    std::map<std::string, std::vector<int>> cam_origwh_inferwh;
    std::vector<std::vector<cv::Point2f>> lanes_impts_;
    // 图片坐标转换到世界坐标, ground points
    std::vector<std::vector<cv::Point2f>> lanes_gpts_; 
    std::vector<std::vector<cv::Point2f>> lane_cnts_;


    rclcpp::Publisher<interface::msg::Objects>::SharedPtr objects_pub_;
    rclcpp::Publisher<interface::msg::MultiLanePoints>::SharedPtr lane_pub_;
    rclcpp::Publisher<interface::msg::Images>::SharedPtr painted_images_pub_;
    std::shared_ptr<rclcpp::Subscription<interface::msg::Images>> sub_;
};

}